

#include "autoaim_node.hpp"

AutoAim::AutoAim(std::string name) : Node(name)
{
    init_global_param();
    armor_detector.init();
    armor_predictor.load_param_path(g_tune_param_path);

    this->declare_parameter("car_id", 5);
    this->car_id = this->get_parameter("car_id").as_int();

    // autoaim_server_ =
    //     this->create_service<interfaces::srv::Armor>(
    //         "auto_aim_srv",
    //         std::bind(&AutoAim::callback_autoaim, this, std::placeholders::_1, std::placeholders::_2));
    sub_autoaim = this->create_subscription<interfaces::msg::Armor>("imgprocess", 1, std::bind(&AutoAim::callback_autoaim, this, std::placeholders::_1));

    // subscriber_Imgpose = this->create_subscription<interfaces::msg::Imgpose>("Imgpose", 15, std::bind(&AutoAim::callback_Imgpose, this, std::placeholders::_1));
    this->readpack_subscriber = this->create_subscription<interfaces::msg::Readpack>("Readpack", 1, std::bind(&AutoAim::readpack_callback, this, std::placeholders::_1));
    // subscriber_time = this->create_subscription<interfaces::msg::Clocknow>("clocknow", 1, std::bind(&AutoAim::callback_clocknow, this, std::placeholders::_1));

    if (!USE_COMM)
    {
        // 实例化图像订阅者，负责订阅取图节点的话题获得图像(debug用)
        sub_debugimage = this->create_subscription<sensor_msgs::msg::Image>("processed_image", 1, std::bind(&AutoAim::image_callback, this, std::placeholders::_1));
    }
    // publisher_Sendpack = this->create_publisher<interfaces::msg::Sendpack>("Sendpack", 10);
    // 实例化图像发布者，调试用
    pub_img = this->create_publisher<sensor_msgs::msg::Image>("processed_img", 10);

    publisher_Sendpack = this->create_publisher<interfaces::msg::Sendpack>("Sendpack", 10);
    pub_pose = this->create_publisher<geometry_msgs::msg::PoseStamped>("pose", 10);
    pub_recorder = this->create_publisher<interfaces::msg::AutoaimRecorder>("autoaim_recorder", 10);
    sub_tovision = this->create_subscription<interfaces::msg::Tovision>("serial_receive", 1, std::bind(&AutoAim::callback_tovision, this, std::placeholders::_1));

    pub_tobase = this->create_publisher<interfaces::msg::Tobase>("tobase", 1);

    // 实例化定时器，用于定时发布
    timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&AutoAim::timer_callback, this));

    // queue_ekf_omega.init(5);
}

void AutoAim::init_global_param()
{
    this->declare_parameter("g_tune_param_path", "src/config/tune_param6.yaml");
    g_tune_param_path = this->get_parameter("g_tune_param_path").as_string();

    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);
    if (!file.isOpened())
    {
        RCLCPP_INFO(this->get_logger(), "can't open camera config file.");
        abort();
    }
    file["synchronizor"] >> synchronizor;

    file["dis_tolerance"] >> dis_tolerance;
    file["dis_confidence"] >> dis_confidence;

    file["v_confidence"] >> v_confidence;
    file["a_confidence"] >> a_confidence;

    file["shoot_delay"] >> shoot_delay;

    file["armor_yaw_offset"] >> armor_yaw_offset;
    file["armor_pitch_offset"] >> armor_pitch_offset;
    file["rune_yaw_offset"] >> rune_yaw_offset;
    file["rune_pitch_offset"] >> rune_pitch_offset;
    file["rune_k"] >> rune_k;
    file["rune_low_pitch"] >> rune_low_pitch;
    file["send_armor_pitch_default"] >> send_armor_pitch_default;
    file["send_armor_yaw_default"] >> send_armor_yaw_default;
    file["send_rune_pitch_offset"] >> send_rune_pitch_offset;
    file["send_rune_yaw_offset"] >> send_rune_yaw_offset;
    file["rune_trajectory_k"] >> rune_trajectory_k;

    file["burst_speed"] >> burst_speed;

    // 旋转前哨站
    file["dynatic_shoot_delay"] >> dynatic_shoot_delay;
    file["dynatic_shoot_proportion"] >> dynatic_shoot_proportion;
    file["dynatic_yaw_offset"] >> dynatic_yaw_offset;
    file["dynatic_pitch_offset"] >> dynatic_pitch_offset;
    file["dynatic_roll_offset"] >> dynatic_roll_offset;
    file["send_dynatic_pitch_offset"] >> send_dynatic_pitch_offset;
    file["send_dynatic_yaw_offset"] >> send_dynatic_yaw_offset;

    file["X_OFFSET"] >> X_OFFSET;
    file["Y_OFFSET"] >> Y_OFFSET;
    file["Z_OFFSET"] >> Z_OFFSET;
    armor_predictor.offset = (cv::Mat_<double>(3, 1) << X_OFFSET / 1000.0, Y_OFFSET / 1000.0, Z_OFFSET / 1000.0);

    file["bullet_m"] >> bullet_m;
    file["bullet_k"] >> bullet_k;
    file["bullet_r"] >> bullet_r;

    file["outpost_height"] >> outpost_height;
    file["robot_height"] >> robot_height;

    file["R_00"] >> this->armor_predictor.kalman.R(0, 0);
    file["R_11"] >> this->armor_predictor.kalman.R(1, 1);
    file["R_22"] >> this->armor_predictor.kalman.R(2, 2);
    file["Q_00"] >> this->armor_predictor.kalman.Q(0, 0);
    file["Q_11"] >> this->armor_predictor.kalman.Q(1, 1);
    file["Q_22"] >> this->armor_predictor.kalman.Q(2, 2);
    file["Q_33"] >> this->armor_predictor.kalman.Q(3, 3);
    file["Q_44"] >> this->armor_predictor.kalman.Q(4, 4);
    file["Q_55"] >> this->armor_predictor.kalman.Q(5, 5);
    file["Q_66"] >> this->armor_predictor.kalman.Q(6, 6);
    file["Q_77"] >> this->armor_predictor.kalman.Q(7, 7);
    // std::cout << this->armor_predictor.kalman.R(0, 0) << std::endl;

    this->declare_parameter("workspace_path", "");
    workspace_path = this->get_parameter("workspace_path").as_string();

    this->declare_parameter("g_camera_param_path", "src/config/camera_param6.xml");
    std::string g_camera_param_path = this->get_parameter("g_camera_param_path").as_string();
    cv::FileStorage camera_param_file;
    camera_param_file.open(g_camera_param_path, cv::FileStorage::READ);
    if (!camera_param_file.isOpened())
    {
        RCLCPP_INFO(this->get_logger(), "open camera_param.yaml failed.%s", g_camera_param_path.c_str());
        abort();
    }
    camera_param_file["camera_internal_matrix"] >> g_camera_internal_matrix;
    camera_param_file["distortion_coeff"] >> g_distortion_coeff;

    armor_predictor.cameraMatrix = g_camera_internal_matrix;
    armor_predictor.distCoeffs = g_distortion_coeff;

    this->declare_parameter("g_other_param_path", "src/config/camera_param6.xml");
    g_other_param_path = this->get_parameter("g_other_param_path").as_string();

    // 装甲板识别
    cv::FileStorage other_param_file(g_other_param_path, cv::FileStorage::READ);
    cv::FileNode armor_detect_node = other_param_file["armor_detect"];
    cv::FileNode workspace_node = other_param_file["workspace"];

    USE_COMM = workspace_node["USE_COMM"];

    // 是否使用数字分类器
    armor_detector.USE_MODEL = armor_detect_node["USE_MODEL"];
    // 是否使用ROI
    armor_detector.ROI_ENABLE = armor_detect_node["ARMOR_ROI_ENABLE"];

    // 图像预处理相关参数传入
    armor_detector.BLUE_BINARY_THRES = armor_detect_node["BLUE_GREY_THRES"];
    armor_detector.BLUE_SUBTRACT_THRES = armor_detect_node["BLUE_SUBTRACT_THRES"];
    armor_detector.BLUE_CHANNEL_THRESH = armor_detect_node["BLUE_CHANNEL_THRESH"];

    armor_detector.RED_BINARY_THRES = armor_detect_node["RED_GREY_THRES"];
    armor_detector.RED_SUBTRACT_THRES = armor_detect_node["RED_SUBTRACT_THRES"];
    armor_detector.RED_CHANNEL_THRESH = armor_detect_node["RED_CHANNEL_THRESH"];

    armor_detector.KERNEL_SIZE = armor_detect_node["KERNEL_SIZE"];

    // 最大预选数量相关参数传入
    armor_detector.MAX_CANDIDATE_NUM = armor_detect_node["MAX_CANDIDATE_NUM"];

    // 装甲板筛选限定条件相关参数传入
    armor_detector.MAX_LIGHTBAR_AREA = armor_detect_node["MAX_LIGHTBAR_AREA"];
    armor_detector.MIN_LIGHTBAR_AREA = armor_detect_node["MIN_LIGHTBAR_AREA"];
    armor_detector.MIN_LIGHTBAR_RATIO = armor_detect_node["MIN_LIGHTBAR_RATIO"];
    armor_detector.MIN_ASPECT_BIG_RATIO = armor_detect_node["MIN_ASPECT_BIG_RATIO"];
    armor_detector.MAX_ASPECT_BIG_RATIO = armor_detect_node["MAX_ASPECT_BIG_RATIO"];
    armor_detector.MIN_ASPECT_SMALL_RATIO = armor_detect_node["MIN_ASPECT_SMALL_RATIO"];
    armor_detector.MAX_ASPECT_SMALL_RATIO = armor_detect_node["MAX_ASPECT_SMALL_RATIO"];
    armor_detector.MAX_LENGTH_RATIO = armor_detect_node["MAX_LENGTH_RATIO"];
    armor_detector.MAX_LIGHTBAR_ANGLE = armor_detect_node["MAX_LIGHTBAR_ANGLE"];
    armor_detector.MAX_LIGHTBAR_DELTA = armor_detect_node["MAX_LIGHTBAR_DELTA"];
    armor_detector.MAX_ARMOR_ANGLE = armor_detect_node["MAX_ARMOR_ANGLE"];
    this->declare_parameter("g_mv_camera_config_path", "src/config/mv_camera_config6.xml");

    std::string mv_camera_config_path = this->get_parameter("g_mv_camera_config_path").as_string();

    cv::FileStorage file_config(mv_camera_config_path, cv::FileStorage::READ);
    if (!file_config.isOpened())
    {
        RCLCPP_INFO(this->get_logger(), "can't open camera config file.");
        abort();
    }

    file_config["frame_width"] >> frame_width;
    file_config["frame_height"] >> frame_height;

    file_config["frame_width"] >> armor_detector.width;
    file_config["frame_height"] >> armor_detector.height;

    file_config["frame_width"] >> armor_predictor.width;
    file_config["frame_height"] >> armor_predictor.height;

    file.release();
    camera_param_file.release();
    other_param_file.release();
    file_config.release();
}

void AutoAim::time_pair()
{
    int ptr;        // ptr指的是从队尾到队首的第ptr个值
    int length = 4; // 最小二乘法获取的点的数量，取成偶数吧
    // RCLCPP_INFO(this->get_logger(), "armors 时间: %lf\n", time_process);
    for (ptr = 1; ptr < queue_readpack.size; ++ptr) // 先在队列里找到取图附近的时间
    {
        interfaces::msg::Readpack tmp;
        queue_readpack.back(tmp, ptr);
        if (tmp.pose.visual_time < time_process / 1000.0)
        {
            break;
        }
    }

    // RCLCPP_INFO(this->get_logger(), "ptr = %d\n", ptr);

    // interfaces::msg::Pose pose_tmp[length];
    std::vector<interfaces::msg::Pose> pose_tmp(4);

    if (ptr - length / 2 < 1) // 最近时间的边界情况
    {
        // RCLCPP_ERROR(this->get_logger(), "error! 电控姿态包太慢或者识别图像太快！\n");
        for (int i = 0; i < length; ++i)
        {
            interfaces::msg::Readpack tmp;
            queue_readpack.back(tmp, i + 1); // 队尾取
            pose_tmp[i] = tmp.pose;
        }
    }
    else if (ptr + length / 2 - 1 > queue_readpack.size - 1) // 最远时间的边界情况
    {
        // RCLCPP_ERROR(this->get_logger(), "error! 电控姿态队列太短或者识别图像太慢！\n");
        for (int i = 0; i < length; ++i)
        {
            interfaces::msg::Readpack tmp;
            tmp = queue_readpack.values[(queue_readpack.head + i) % queue_readpack.size]; // 队首取
            pose_tmp[i] = tmp.pose;
        }
    }
    else // 正常情况
    {
        for (int i = 0; i < length; ++i)
        {
            interfaces::msg::Readpack tmp;
            queue_readpack.back(tmp, ptr - length / 2 + i);
            pose_tmp[i] = tmp.pose;
        }
    }

    auto Least_Squares_Method = [this](std::vector<interfaces::msg::Pose> &pose_vec, const double &time_to_predict) -> interfaces::msg::Pose
    {
        double a_sin_yaw_hat, a_cos_yaw_hat, a_pitch_hat, a_roll_hat;
        double b_sin_yaw_hat, b_cos_yaw_hat, b_pitch_hat, b_roll_hat;

        double sigma_time_sin_yaw = 0, sigma_time_cos_yaw = 0, sigma_time_pitch = 0, sigma_time_roll = 0;
        double sigma_time_time = 0;
        double sigma_time = 0;
        double sigma_sin_yaw = 0, sigma_cos_yaw = 0, sigma_pitch = 0, sigma_roll = 0;

        std::sort(pose_vec.begin(), pose_vec.end(), [](const interfaces::msg::Pose &p1, const interfaces::msg::Pose &p2) -> bool
                  { return p1.visual_time < p2.visual_time; });
        double base_time;
        if (time_to_predict > pose_vec[0].visual_time)
        {
            base_time = pose_vec[0].visual_time;
        }
        else
        {
            base_time = time_to_predict;
        }
        // RCLCPP_INFO(this->get_logger(), "base time = %lf\n", base_time);

        for (auto &p : pose_vec)
        {
            p.visual_time -= base_time;
        }

        for (auto &p : pose_vec)
        {
            sigma_time_sin_yaw += p.visual_time * sin(p.ptz_yaw * acos(-1.0) / 180.0);
            sigma_time_cos_yaw += p.visual_time * cos(p.ptz_yaw * acos(-1.0) / 180.0);
            sigma_time_pitch += p.visual_time * p.ptz_pitch;
            sigma_time_roll += p.visual_time * p.ptz_roll;

            sigma_time_time += p.visual_time * p.visual_time;
            sigma_time += p.visual_time;

            sigma_sin_yaw += sin(p.ptz_yaw * acos(-1.0) / 180.0);
            sigma_cos_yaw += cos(p.ptz_yaw * acos(-1.0) / 180.0);
            sigma_pitch += p.ptz_pitch;
            sigma_roll += p.ptz_roll;
        }

        // for (auto& p: pose_vec)
        // {
        //     RCLCPP_INFO(this->get_logger(), "time: %lf  ", p.visual_time);
        // }

        // RCLCPP_INFO(this->get_logger(), "sigma_time_time = %lf, sigma_time * sigma_time = %lf, fenmu = %lf\n", sigma_time_time, sigma_time * sigma_time, sigma_time_time - sigma_time * sigma_time / pose_vec.size());

        b_sin_yaw_hat = (sigma_time_sin_yaw - sigma_time * sigma_sin_yaw / pose_vec.size()) / (sigma_time_time - sigma_time * sigma_time / pose_vec.size());
        a_sin_yaw_hat = sigma_sin_yaw / pose_vec.size() - b_sin_yaw_hat * sigma_time / pose_vec.size();

        b_cos_yaw_hat = (sigma_time_cos_yaw - sigma_time * sigma_cos_yaw / pose_vec.size()) / (sigma_time_time - sigma_time * sigma_time / pose_vec.size());
        a_cos_yaw_hat = sigma_cos_yaw / pose_vec.size() - b_cos_yaw_hat * sigma_time / pose_vec.size();

        b_pitch_hat = (sigma_time_pitch - sigma_time * sigma_pitch / pose_vec.size()) / (sigma_time_time - sigma_time * sigma_time / pose_vec.size());
        a_pitch_hat = sigma_pitch / pose_vec.size() - b_pitch_hat * sigma_time / pose_vec.size();

        b_roll_hat = (sigma_time_roll - sigma_time * sigma_roll / pose_vec.size()) / (sigma_time_time - sigma_time * sigma_time / pose_vec.size());
        a_roll_hat = sigma_roll / pose_vec.size() - b_roll_hat * sigma_time / pose_vec.size();

        interfaces::msg::Pose result;

        double sin_yaw_hat = b_sin_yaw_hat * (time_to_predict - base_time) + a_sin_yaw_hat;
        double cos_yaw_hat = b_cos_yaw_hat * (time_to_predict - base_time) + a_cos_yaw_hat;
        if (sin_yaw_hat >= 1)
        {
            result.ptz_yaw = 90;
        }
        else if (sin_yaw_hat <= -1)
        {
            result.ptz_yaw = -90;
        }
        else
        {
            result.ptz_yaw = asin(sin_yaw_hat);
            if (cos_yaw_hat < 0)
            {
                result.ptz_yaw = (-result.ptz_yaw + ((result.ptz_yaw > 0) ? 1 : (-1)) * acos(-1.0));
            }
            result.ptz_yaw *= 180.0 / acos(-1.0);
        }
        // RCLCPP_INFO(this->get_logger(), "b_pitch_hat = %lf, a_pitch_hat = %lf, b_roll_hat = %lf, a_roll_hat = %lf\n", b_pitch_hat, a_pitch_hat, b_roll_hat, a_roll_hat);
        result.ptz_pitch = b_pitch_hat * (time_to_predict - base_time) + a_pitch_hat;
        result.ptz_roll = b_roll_hat * (time_to_predict - base_time) + a_roll_hat;
        result.visual_time = time_to_predict;

        return result;
    }; // 最小二乘法

    interfaces::msg::Pose result;
    result = Least_Squares_Method(pose_tmp, time_process / 1000.0);

    // RCLCPP_INFO(this->get_logger(), "yaw: %f, pitch: %f, roll: %f\n", result.ptz_yaw, result.ptz_pitch, result.ptz_roll);

    // std::map<double, interfaces::msg::Readpack::SharedPtr>::iterator it = readpack_dictionary.find(round(time_process));
    // if (!(round(time_process) == round(it->second->pose.visual_time * 1000)))
    // {
    //     std::map<double, interfaces::msg::Readpack::SharedPtr>::iterator it_up = readpack_dictionary.find(round(time_process) + 1);
    //     if (!(round(time_process) + 1 == round(it_up->second->pose.visual_time * 1000)))
    //     {
    //         std::map<double, interfaces::msg::Readpack::SharedPtr>::iterator it_up = readpack_dictionary.find(round(time_process) + 2);
    //     }
    //     std::map<double, interfaces::msg::Readpack::SharedPtr>::iterator it_down = readpack_dictionary.find(round(time_process) - 1);
    //     if (!(round(time_process) - 1 == round(it_down->second->pose.visual_time * 1000)))
    //     {
    //         std::map<double, interfaces::msg::Readpack::SharedPtr>::iterator it_down = readpack_dictionary.find(round(time_process) - 2);
    //     }
    //     interfaces::msg::Pose result;
    //     result.visual_time = round(time_process)/1000;
    //     linear_predict_pose(result,it_up->second->pose,it_down->second->pose,synchronizor);
    //     it = it_up;
    //     it->second->pose = result;
    // }

    pose.ptz_pitch = result.ptz_pitch;
    pose.ptz_roll = result.ptz_roll;
    pose.ptz_yaw = result.ptz_yaw;

    pose.mcu_time = round(time_process);
    // RCLCPP_INFO(this->get_logger(), "time_process = %f", time_process);
    work_msg.mode = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].mode;
    mode = work_msg.mode;
    // RCLCPP_INFO(this->get_logger(), "mode: %d\n", mode);
    tmp_bullet_speed = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].bullet_speed;

    if (last_bullet_speed != tmp_bullet_speed)
    {
        work_msg.bullet_speed = tmp_bullet_speed;
        calculate_bullet_speed(work_msg.bullet_speed);
    }

    last_bullet_speed = tmp_bullet_speed;

    work_msg.enemy_color = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].enemy_color;
    work_msg.enemy_number = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].enemy_number;
    work_msg.isRightMouseButtonClicked = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].rightclick;
    work_msg.fired = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].fired;
    work_msg.request_fire = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size].request_fire;

    autoaim_recorder.pose_ptz_yaw = pose.ptz_yaw;
    autoaim_recorder.pose_ptz_pitch = pose.ptz_pitch;
    autoaim_recorder.pose_ptz_roll = pose.ptz_roll;
    autoaim_recorder.pose_mcu_time = pose.mcu_time;
    autoaim_recorder.mode = mode;
    autoaim_recorder.bullet_speed = work_msg.bullet_speed;
    autoaim_recorder.enemy_color = work_msg.enemy_color;
    autoaim_recorder.enemy_number = work_msg.enemy_number;
    autoaim_recorder.is_right_clicked = work_msg.isRightMouseButtonClicked;
    autoaim_recorder.fired = work_msg.fired;
    autoaim_recorder.request_fire = work_msg.request_fire;

    // RCLCPP_INFO(this->get_logger(), "ptz_pitch=%lf,ptz_roll=%lf,ptz_yaw=%lf,mcu_time=%ld,mode=%d,bullet_speed=%f,enemy_color=%d,enemy_number=%d,isRightMouseButtonClicked=%d,fired=%d,request_fire=%d\n", pose.ptz_pitch, pose.ptz_roll, pose.ptz_yaw, pose.mcu_time, work_msg.mode, work_msg.bullet_speed, work_msg.enemy_color, work_msg.enemy_number, work_msg.isRightMouseButtonClicked, work_msg.fired, work_msg.request_fire);
}

void AutoAim::calculate_bullet_speed(double &bullet_speed)
{
    std::filesystem::create_directories(workspace_path + "/src/autoaim/save/");
    std::ofstream f_tmp(workspace_path + "/src/autoaim/save/bullet_speed.csv");
    f_tmp.close();
    std::ifstream fin(workspace_path + "/src/autoaim/save/bullet_speed.csv");
    if (!fin.is_open())
    {
        RCLCPP_ERROR(this->get_logger(), "Error while reading bullet_speed.csv!");
        if (10 > bullet_speed || 40 < bullet_speed)
        {
            bullet_speed = 22;
        }
        return;
    }

    CircleQueue<double, 2 + 1> tmp_queue; // 里面可以存2个数据
    std::string line;
    while (getline(fin, line))
    {
        if (!line.empty())
        {
            tmp_queue.push(std::stod(line)); // 将所有弹速读入
        }
    }
    fin.close();
    if (10 <= bullet_speed && 40 >= bullet_speed)
    {
        tmp_queue.push(bullet_speed);
    }

    if (0 == tmp_queue.length && (10 > bullet_speed || 40 < bullet_speed)) // 特殊情况处理
    {
        bullet_speed = 22;
        return;
    }

    double sum = 0;
    for (int i = 0; i < tmp_queue.length; ++i)
    {
        double tmp = 0;
        tmp_queue.back(tmp, i + 1);
        sum += tmp;
    }
    sum /= tmp_queue.length;
    bullet_speed = sum; // 取平均得弹速

    std::ofstream fout(workspace_path + "/src/autoaim/save/bullet_speed.csv", std::ios::trunc);
    if (!fout.is_open())
    {
        RCLCPP_ERROR(this->get_logger(), "Error while writing bullet_speed.csv!");
        return;
    }
    for (int i = 0; i < tmp_queue.length; ++i)
    {
        double tmp = 0;
        tmp_queue.back(tmp, tmp_queue.length - i);
        fout << tmp << '\n';
    }
    fout.close();
}

void AutoAim::linear_predict_pose(interfaces::msg::Pose &result, interfaces::msg::Pose &pose1, interfaces::msg::Pose &pose2, int synchronizor)
{
    // 以 pose1.visual_time 为时间原点
    // 计算各姿态分量的增量
    if ((pose2.ptz_yaw - pose1.ptz_yaw) > 150)
    {
        pose1.ptz_yaw += 360;
    }
    else if ((pose2.ptz_yaw - pose1.ptz_yaw) < -150)
    {
        pose1.ptz_yaw -= 360;
    }

    double delta_ptz_pitch = pose2.ptz_pitch - pose1.ptz_pitch;
    double delta_ptz_yaw = pose2.ptz_yaw - pose1.ptz_yaw;
    double delta_ptz_roll = pose2.ptz_roll - pose1.ptz_roll;
    // 计算 ptz-t 直线的斜率
    double delta_t = (pose2.visual_time - pose1.visual_time);
    double k_ptz_pitch = 1.0 * delta_ptz_pitch / delta_t;
    double k_ptz_yaw = 1.0 * delta_ptz_yaw / delta_t;
    double k_ptz_roll = 1.0 * delta_ptz_roll / delta_t;
    // 得到直线即为 result.ptz = k * result.visual_time + pose1.ptz
    result.ptz_pitch = k_ptz_pitch * (result.visual_time - pose1.visual_time + synchronizor) + pose1.ptz_pitch;
    result.ptz_yaw = k_ptz_yaw * (result.visual_time - pose1.visual_time + synchronizor) + pose1.ptz_yaw;
    result.ptz_roll = k_ptz_roll * (result.visual_time - pose1.visual_time + synchronizor) + pose1.ptz_roll;
}

void AutoAim::image_callback(const sensor_msgs::msg::Image::SharedPtr debug_image)
{
    cv_bridge::CvImage::ConstPtr cv_temp = cv_bridge::toCvCopy(debug_image);
    // 得到原图（仅有图像信息，所以时间戳需要提前拿出来）
    testimg = cv_temp->image;
    // 获取图像宽高
    img_width = testimg.cols;
    img_height = testimg.rows;

    // cv::imshow("debug_autoaim", testimg);
    // cv::waitKey(1);
}

void AutoAim::readpack_callback(const interfaces::msg::Readpack::SharedPtr readpack)
{
    double armor_yaw_offset = 0, armor_pitch_offset = 0, armor_roll_offset = 0;
    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);

    file["armor_yaw_offset"] >> armor_yaw_offset;
    file["armor_pitch_offset"] >> armor_pitch_offset;
    file["armor_roll_offset"] >> armor_roll_offset;

    file.release(); // 热调参

    readpack->pose.ptz_yaw = readpack->pose.ptz_yaw + armor_yaw_offset;
    readpack->pose.ptz_roll = readpack->pose.ptz_roll + armor_roll_offset;
    readpack->pose.ptz_pitch = readpack->pose.ptz_pitch + armor_pitch_offset;

    queue_readpack.push(*readpack);
    interfaces::msg::Readpack head, tail;
    head = queue_readpack.values[queue_readpack.head];
    tail = queue_readpack.values[(queue_readpack.tail + queue_readpack.size - 1) % queue_readpack.size];
    // queue_readpack.back(head, queue_readpack.size - 2);
    // queue_readpack.back(tail, 0);
    // RCLCPP_INFO(this->get_logger(), "queue head time: %lf, tail time: %lf", head.pose.visual_time, tail.pose.visual_time);//单位：秒

    // readpack_dictionary.insert(std::pair<double, interfaces::msg::Readpack::SharedPtr>(round(readpack->pose.visual_time * 1000), readpack));
    // if (imgpose_dictionary.size() > 50)
    // {
    //     imgpose_dictionary.erase(imgpose_dictionary.begin());
    // }

    // // RCLCPP_INFO(get_logger(), "进入readpack回调函数");

    // temp_imgpose.mode = readpack->mode;
    // temp_imgpose.bullet_speed = readpack->bullet_speed;
    // temp_imgpose.enemy_color = readpack->enemy_color;
    // temp_imgpose.enemy_number = readpack->enemy_number;
    // temp_imgpose.rightclick = readpack->rightclick;
    // temp_imgpose.fired = readpack->fired;
    // temp_imgpose.request_fire = readpack->request_fire;

    // // std::mutex pose_queue_metux;
    // img_pose_queue_metux.lock();
    // img_pose_queue.push(readpack->pose);
    // img_pose_queue_metux.unlock();
}

// void AutoAim::callback_clocknow(const interfaces::msg::Clocknow::SharedPtr clock_now_temp)
// {
//     // RCLCPP_INFO(get_logger(), "成功获取发送节点当前时间");

//     clock_now_new=clock_now_temp->clocknow;
//     RCLCPP_INFO(get_logger(), "成功获取发送节点当前时间: %lf",clock_now_new);

//     // RCLCPP_INFO(get_logger(), "成功获取发送节点当前时间");

// }

void AutoAim::callback_autoaim(interfaces::msg::Armor::SharedPtr armors_in)
{
    // RCLCPP_INFO(this->get_logger(), "进入callback");
    auto tmp_time = get_clock()->now();
    start_time = tmp_time.nanoseconds()/1e6;

    std::cout << "start_time: " << start_time - 1722868763000;

    convert(armors_in);
    time_pair();

    // std::cout << "572" << std::endl;
    if (3 == car_id)
    {
        autoaim_recorder.tov.control_mode = this->control_mode;
        autoaim_recorder.tov.hit_priority = this->queue_hit_priority;
        autoaim_recorder.tov.dishit = this->queue_dishit;
    }
    // std::cout << "579" << std::endl;

    // RCLCPP_INFO(autoaim_node->get_logger(), "进入callback_autoaim回调函数");

    // std::cout <<" mode test_1:"  << mode<<std::endl;
    // mode = 6;
    // RCLCPP_INFO(autoaim_node->get_logger(), "mode: %d", mode);
    double rate = 0;
    bool flg_if_uses_aim_mode = 0;
    if (4 != mode && 5 != mode)
    {
        dynamic_hit_outposter.outpost_description.valid = false;
        armor_num = 4;
    }
    else
    {
        armor_num = 3;
    }

    switch (mode)
    {
    case 1: // 自瞄
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "进入callback_autoaim回调函数");
        armorFunc(armors_in);

        tb.time = armors_in->time * 1000;
        break;
    }
    case 4: // 跟随击打前哨站
    {
        if (1 == car_id || 6 == car_id)
        {
            fix_hit_outpost();
        }
        else
        {
            dynamic_follow_hit_outpost();
            // if (3 == car_id)
            // {
            //     dynamic_follow_hit_outpost(ROI);
            // if (3 == car_id)
            // {
            //     // armorFunc(armors_in, ROI);
            //     if (armor_detector.armorFound)
            //     {
            //         queue_armor_found.push(1);
            //     }
            //     else
            //     {
            //         queue_armor_found.push(0);
            //     }

            //     if (!dynamic_hit_outposter.outpost_description.valid)
            //     {
            //         RCLCPP_INFO(this->get_logger(), "invalid!");
            //         flg_if_uses_aim_mode = 1;
            //     }
            //     else
            //     {
            //         if (queue_armor_found.length < 100)
            //         {
            //             RCLCPP_INFO(this->get_logger(), "not full!");
            //             flg_if_uses_aim_mode = 1;
            //         }
            //         else
            //         {
            //             RCLCPP_INFO(this->get_logger(), "full!");
            //             int sum = 0;
            //             for (int i = 1; i <= queue_armor_found.length; ++i)
            //             {
            //                 bool tmp = 0;
            //                 queue_armor_found.back(tmp, i);
            //                 sum += tmp;
            //             }

            //             rate = (double)sum / queue_armor_found.length;
            //             RCLCPP_INFO(this->get_logger(), "rate: %lf", rate);

            //             if (rate > 0.4)
            //             {
            //                 flg_if_uses_aim_mode = 0;
            //             }
            //             else
            //             {
            //                 flg_if_uses_aim_mode = 1;
            //             }
            //         }
            //     }
            // }
            // else
            // {
            //     dynamic_follow_hit_outpost(ROI);
            //     if (3 == car_id)
            //     {
            //         // armorFunc(armors_in, ROI);
            //         if (armor_detector.armorFound)
            //         {
            //             queue_armor_found.push(1);
            //         }
            //         else
            //         {
            //             queue_armor_found.push(0);
            //         }

            //         if (!dynamic_hit_outposter.outpost_description.valid)
            //         {
            //             RCLCPP_INFO(this->get_logger(), "invalid!");
            //             flg_if_uses_aim_mode = 1;
            //         }
            //         else
            //         {
            //             if (queue_armor_found.length < 100)
            //             {
            //                 RCLCPP_INFO(this->get_logger(), "not full!");
            //                 flg_if_uses_aim_mode = 1;
            //             }
            //             else
            //             {
            //                 RCLCPP_INFO(this->get_logger(), "full!");
            //                 int sum = 0;
            //                 for (int i = 1; i <= queue_armor_found.length; ++i)
            //                 {
            //                     bool tmp = 0;
            //                     queue_armor_found.back(tmp, i);
            //                     sum += tmp;
            //                 }

            //                 rate = (double)sum / queue_armor_found.length;
            //                 RCLCPP_INFO(this->get_logger(), "rate: %lf", rate);

            //                 if (rate > 0.4)
            //                 {
            //                     flg_if_uses_aim_mode = 0;
            //                 }
            //                 else
            //                 {
            //                     flg_if_uses_aim_mode = 1;
            //                 }
            //             }
            //         }
            //     }
            // }
            // break;
            // }
            // break;
        }
        break;
    }
    case 5: // 固定击打前哨站
    {
        if (1 == car_id || 6 == car_id)
        {
            fix_hit_outpost();
        }
        else
        {
            dynamic_follow_hit_outpost();
        }
        break;
    }
    case 6: // 灯光辅助击打前哨站
    {
        armorFunc(armors_in);
        break;
    }
    default:
    {
        RCLCPP_ERROR(autoaim_node->get_logger(), "error work mode!");
        break;
    }
    }

    auto time_tmp = get_clock()->now();

    double t_init = time_tmp.nanoseconds() / 1e6;

    std::cout << ", t_init: " << t_init - 1722868763000;

    // t_init = armor_predictor.curr_target_time + 20;//测试用
    // double t_init=clock_now_new;
    // std::cout <<" mode test_2:"  <<mode<< std::endl;
    switch (mode)
    {
        // case Mode::MODE_SYNCHRONIZOR_CALIB: setSyncPredData(send_pack,clock_now); break;
        // case Mode::MODE_TARGETSOLVE_CALIB: setTargetSolvePredData(send_pack,clock_now); break;
        // std::cout << "test_mode" << std::endl;
    case 1:
    {
        setArmorPredictdata(send_pack, round(t_init));
        break;
    }
    case 4:
    {
        if (1 == car_id || 6 == car_id)
        {
            setFixHitOutpostNew(send_pack, round(t_init));
        }
        else
        {
            // if (3 == car_id)
            // {
            //     if (1 == flg_if_uses_aim_mode)
            //     {
            //         armor_predictor.predTarget(work_msg.bullet_speed, round(t_init), send_pack);
            //     }
            //     else
            //     {
            //         setDynamicHitOutpostNew(send_pack, round(t_init));
            //     }
            // }
            // else
            // {
            //     if (3 == car_id)
            //     {
            //         if (1 == flg_if_uses_aim_mode)
            //         {
            //             armor_predictor.predTarget(work_msg.bullet_speed, round(t_init), send_pack);
            //         }
            //         else
            //         {
            setDynamicHitOutpostNew(send_pack, round(t_init));
            //         }
            //     }
            //     else
            //     {
            //     if (3 == car_id)
            //     {
            //         if (1 == flg_if_uses_aim_mode)
            //         {
            //             armor_predictor.predTarget(work_msg.bullet_speed, round(t_init), send_pack);
            //         }
            //         else
            //         {
            //         setDynamicHitOutpostNew(send_pack, round(t_init));
            //     }
            // }
            //     else
            //     {
            //         setDynamicHitOutpostNew(send_pack, round(t_init));
            //     }
            // }
            // }
            // break;
        }
        break;
    }
    case 5:
    {
        if (1 == car_id || 6 == car_id)
        {
            setFixHitOutpostNew(send_pack, round(t_init));
        }
        else
        {
            setDynamicHitOutpostNew(send_pack, round(t_init));
        }
        break;
    }
    case 6:
    {
        setArmorPredictdata(send_pack, round(t_init));
        break;
    }
    default:
    {
        break;
    }
        // std::cout<<"pred_pitch:"<<send_pack.pred_pitch<<std::endl;
    }

    interfaces::msg::Sendpack sendpack_out;

    double send_armor_yaw_default = 0, send_armor_pitch_default = 0;
    cv::FileStorage fs;
    fs.open(g_tune_param_path, cv::FileStorage::READ);
    fs["send_armor_yaw_default"] >> send_armor_yaw_default;
    fs["send_armor_pitch_default"] >> send_armor_pitch_default;
    fs.release(); // 热调参



    sendpack_out.mode = mode;
    sendpack_out.clock = t_init / 1000;
    // sendpack_out.pred_pitch = send_pack.pred_pitch + o_pitch_dep / M_PI * 180;



    // 默认方法
    // sendpack_out.pred_pitch = send_pack.pred_pitch + send_armor_pitch_default;
    // if (4 == car_id)
    // {
    //     sendpack_out.pred_pitch += 9.182 * pow(tmp_xy_dis, -1.093);
    // }
    // // sendpack_out.pred_yaw = send_pack.pred_yaw + o_yaw_dep / M_PI * 180;
    // sendpack_out.pred_yaw = send_pack.pred_yaw + send_armor_yaw_default;


    //考虑roll的方法
    double y = send_pack.pred_yaw * M_PI / 180;
    double p = send_pack.pred_pitch * M_PI / 180;
    double r = pose.ptz_roll * M_PI / 180;
    double oy = send_armor_yaw_default * M_PI / 180;
    double op = send_armor_pitch_default * M_PI / 180;
    cv::Mat X_0 = (cv::Mat_<double>(3, 1) << 0, 1, 0);
    cv::Mat P_y = (cv::Mat_<double>(3, 3) << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1);
    cv::Mat P_p = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, cos(p), -sin(p), 0, sin(p), cos(p));
    cv::Mat P_r = (cv::Mat_<double>(3, 3) << cos(r), 0, sin(r), 0, 1, 0, -sin(r), 0, cos(r));
    cv::Mat P_oy = (cv::Mat_<double>(3, 3) << cos(oy), -sin(oy), 0, sin(oy), cos(oy), 0, 0, 0, 1);
    cv::Mat P_op = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, cos(op), -sin(op), 0, sin(op), cos(op));
    cv::Mat X_l = P_y * P_p * P_r * P_oy * P_op * X_0;
    double p_dep = asin(X_l.at<double>(2, 0));
    double y_dep = atan2(-X_l.at<double>(0, 0)/cos(p_dep), X_l.at<double>(1, 0)/cos(p_dep));

    sendpack_out.pred_yaw = y_dep * 180 / M_PI;
    sendpack_out.pred_pitch = p_dep * 180 / M_PI;
    if (4 == car_id)
    {
        sendpack_out.pred_pitch += 9.182 * pow(tmp_xy_dis, -1.093);
    }



    sendpack_out.yaw_resolution = send_pack.yaw_resolution;
    sendpack_out.pitch_resolution = send_pack.pitch_resolution;
    if (0 == car_id)
    {
        sendpack_out.yaw_resolution = 1000;
        sendpack_out.pitch_resolution = 1000;
    }
    sendpack_out.yaw_palstance = send_pack.yaw_palstance;
    sendpack_out.pitch_palstance = send_pack.pitch_palstance;
    // sendpack_out.pitch_palstance = 0;
    sendpack_out.target_found = send_pack.target_found;
    sendpack_out.fire = send_pack.fire;
    // RCLCPP_INFO(this->get_logger(), "fire: %d", sendpack_out.fire);
    sendpack_out.burst_shoot = send_pack.burstShoot;
    sendpack_out.isproperly = 1;
    sendpack_out.wannfiretime = send_pack.wann_fire_time;
    // RCLCPP_INFO(this->get_logger(), "predict: %lf, %lf\n", sendpack_out.pred_yaw, sendpack_out.pred_pitch);
    // if (send_pack.target_found==1&&send_pack.pred_pitch<10&&send_pack.pred_yaw<10)
    // std::cout << "error!!!!!!!!!!!!!!!!!!!!!" << std::endl;

    publisher_Sendpack->publish(sendpack_out);

    autoaim_recorder.time = this->now();
    pub_recorder->publish(autoaim_recorder);

    tmp_time = get_clock()->now();
    end_time = tmp_time.nanoseconds()/1e6;
    std::cout << ", end_time: " << end_time - 1722868763000 << std::endl;
    // std::cout << "run time: " << end_time - start_time << std::endl;
}

void AutoAim::callback_tovision(interfaces::msg::Tovision::SharedPtr tovision_in)
{
    // RCLCPP_INFO(this->get_logger(), "895");
    control_mode = tovision_in->control_mode;
    queue_hit_priority = tovision_in->hit_priority;
    if (0 != queue_hit_priority.size())
    {
        queue_dishit = tovision_in->dishit;
        int index_6 = 0, index_7 = 0;
        // RCLCPP_INFO(this->get_logger(), "900");
        for (int i = 0; i < queue_hit_priority.size(); ++i)
        {
            if (6 == queue_hit_priority[i])
            {
                index_7 = i;
            }
            else if (7 == queue_hit_priority[i])
            {
                index_6 = i;
            }
            else if (0 == queue_hit_priority[i])
            {
                queue_hit_priority[i] = 8;
            }
        }
        // RCLCPP_INFO(this->get_logger(), "916");
        unsigned char tmp = queue_hit_priority[index_6];
        queue_hit_priority[index_6] = queue_hit_priority[index_7];
        queue_hit_priority[index_7] = tmp; // 转化成我们认为的装甲板数字
        // RCLCPP_INFO(this->get_logger(), "920");

        tmp = queue_dishit[6];
        queue_dishit[6] = queue_dishit[7];
        queue_dishit[7] = tmp;

        queue_dishit.push_back(tovision_in->dishit[0]);
    }
    // RCLCPP_INFO(this->get_logger(), "895");

    // RCLCPP_INFO(autoaim_node->get_logger(), "control_mode: %d", autoaim_node->control_mode);
    // for (int i = 0; i < queue_hit_priority.size(); ++i)
    // {
    //     RCLCPP_INFO(this->get_logger(), "hit_pri: %d", queue_hit_priority[i]);
    // }
    // for (int i = 0; i < queue_dishit.size(); ++i)
    // {
    //     RCLCPP_INFO(this->get_logger(), "dis_hit: %d", queue_dishit[i]);
    // }
}

void AutoAim::convert(interfaces::msg::Armor::SharedPtr armors_in)
{
    armors.clear();
    time_process = armors_in->time * 1000;
    // autoaim_node->autoaim_recorder.armor_num1  = armors_in->nums;
    // printf("time_process%lf\n",time_process);
    for (int i = 0; i < armors_in->nums; i++)
    {
        Armor armor_temp;
        armor_temp.if_use_net = armors_in->net;
        if (armors_in->net)
        {
            armor_temp.coord_points[0] = cv::Point2f(armors_in->cord_x[0 + 4 * i], armors_in->cord_y[0 + 4 * i]);
            armor_temp.coord_points[1] = cv::Point2f(armors_in->cord_x[3 + 4 * i], armors_in->cord_y[3 + 4 * i]);
            armor_temp.coord_points[2] = cv::Point2f(armors_in->cord_x[2 + 4 * i], armors_in->cord_y[2 + 4 * i]);
            armor_temp.coord_points[3] = cv::Point2f(armors_in->cord_x[1 + 4 * i], armors_in->cord_y[1 + 4 * i]);
            std::vector<cv::Point2f> lbar(4);
            std::vector<cv::Point2f> rbar(4);
            lbar[0] = armor_temp.coord_points[0];
            lbar[1] = armor_temp.coord_points[0];
            lbar[2] = armor_temp.coord_points[3];
            lbar[3] = armor_temp.coord_points[3];
            rbar[0] = armor_temp.coord_points[1];
            rbar[1] = armor_temp.coord_points[1];
            rbar[2] = armor_temp.coord_points[2];
            rbar[3] = armor_temp.coord_points[2];
            armor_temp.lbar_rect = cv::minAreaRect(lbar);
            armor_temp.rbar_rect = cv::minAreaRect(rbar);
        }
        else
        {
            armor_temp.lbar_rect.angle = armors_in->angles[2 * i];
            armor_temp.rbar_rect.angle = armors_in->angles[2 * i + 1];
            armor_temp.lbar_rect.center.x = armors_in->center_xs[2 * i];
            armor_temp.rbar_rect.center.x = armors_in->center_xs[2 * i + 1];
            armor_temp.lbar_rect.center.y = armors_in->center_ys[2 * i];
            armor_temp.rbar_rect.center.y = armors_in->center_ys[2 * i + 1];
            armor_temp.lbar_rect.size.height = armors_in->heights[2 * i];
            armor_temp.rbar_rect.size.height = armors_in->heights[2 * i + 1];
            armor_temp.lbar_rect.size.width = armors_in->widths[2 * i];
            armor_temp.rbar_rect.size.width = armors_in->widths[2 * i + 1];
        }

        armor_temp.target.x = armors_in->x[i];
        armor_temp.target.y = armors_in->y[i];
        armor_temp.target.z = armors_in->z[i];

        std::vector<cv::Point> armor_vertices;
        Util::extractPoints(armor_temp.lbar_rect, armor_vertices);
        Util::extractPoints(armor_temp.rbar_rect, armor_vertices);
        armor_temp.armor_rect = cv::minAreaRect(armor_vertices);
        adjustArmorRect(armor_temp.armor_rect);

        armor_temp.target.x += X_OFFSET / 1000.0;
        armor_temp.target.y += Y_OFFSET / 1000.0;
        armor_temp.target.z += Z_OFFSET / 1000.0;
        Util::wheel2land(armor_temp.target, pose); /* 转换到大地坐标系 */

        armor_temp.distance = armors_in->distances[i];
        armor_temp.isBigArmor = armors_in->isbigarmors[i];
        armor_temp.classifier_num = armors_in->class_nums[i];
        // if (6 == armor_temp.classifier_num)
        // {
        //     armor_temp.classifier_num = 7;
        // }
        // else if (7 == armor_temp.classifier_num)
        // {
        //     armor_temp.classifier_num = 6;
        // }
        // RCLCPP_INFO(autoaim_node->get_logger(), "classifier_num: %d", armor_temp.classifier_num);

        armor_temp.target.yaw = Util::getYawForPTZ(armor_temp.target);

        // if (!armors_in->net)
        // {
        //     armors.push_back(armor_temp);
        // }
        // else
        // {
        //     double rate = (cv::norm(armor_temp.coord_points[0]-armor_temp.coord_points[1])+cv::norm(armor_temp.coord_points[2]-armor_temp.coord_points[3]))/(cv::norm(armor_temp.coord_points[0]-armor_temp.coord_points[3])+cv::norm(armor_temp.coord_points[2]-armor_temp.coord_points[1]));
        //     RCLCPP_INFO(this->get_logger(), "rate: %lf, classifier: %d", rate, armor_temp.classifier_num);
        //     if (7 == armor_temp.classifier_num)
        //     {
        //         if (2.3 < rate)
        //         {
        //             armors.push_back(armor_temp);
        //         }
        //         else
        //         {
        //             RCLCPP_INFO(this->get_logger(), "筛掉！");
        //         }
        //     }
        //     else
        //     {
        //         armors.push_back(armor_temp);
        //     }
        // }

        double min_armor_height = std::min(cv::norm(armor_temp.coord_points[3]-armor_temp.coord_points[0]), cv::norm(armor_temp.coord_points[2]-armor_temp.coord_points[1]));
        double min_armor_width = std::min(cv::norm(armor_temp.coord_points[1]-armor_temp.coord_points[0]), cv::norm(armor_temp.coord_points[2]-armor_temp.coord_points[3]));
        // if (min_armor_height / min_armor_width < 0.5)
        // {
            armors.push_back(armor_temp);
        // }

        // std::cout << "convert finished" << std::endl;
    }
    autoaim_recorder.armor_num1 = armors.size();
    // RCLCPP_INFO(autoaim_node->get_logger(), "num of armors: %d", armors.size());

    // fps = armors_in->fps;
    //  std::cout << "fps is " << fps << std::endl;
}

void AutoAim::armorFunc(interfaces::msg::Armor::SharedPtr armors_in)
{
    // RCLCPP_INFO(autoaim_node->get_logger(), "611");
    //          if (armors_in->nums)std::cout << "armors_in :"<<armors_in->nums << std::endl;
    //     std::cout << "test89" << std::endl;
    //  加一个转换函数.....
    //  convert(armors_in);
    //  time_pair();
    //  if (armors.size())std::cout << "armors" <<armors.size() << std::endl;
    //  旋转方向，每次重置判断此时旋转方向是否有效
    ArmorDir dir = ArmorDir::UNKNOWN;
    // RCLCPP_INFO(autoaim_node->get_logger(), "实时性检测：%d",anti_gyror.If_gyro_description_updated(pose.mcu_time));
    if (1 == mode && anti_gyror.If_gyro_description_updated(pose.mcu_time) && anti_gyror.gyro_description.valid) // 判断是否具有实时性
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "进行赋值");
        dir = anti_gyror.gyro_description.dir;
    }
    // 每次重置数字
    work_msg.enemy_number = 0;
    armor_detector.run(armors, work_msg, dir, pose, armor_predictor.ekf_yaw_v);
    if (1 == armor_detector.is_balance_infantry)
    {
        armor_num = 2;
    }
    else
    {
        armor_num = 4;
    }
    armor_predictor.get_tracker_state(armor_detector.curr_state, armor_detector.next_state);

    // save_readpack(work_msg);
    autoaim_node->autoaim_recorder.clearmode = 0;
    if (armor_detector.flag_track_car_failed)
    {
        if (6 != mode)
        {
            anti_gyror.clear_if_car_switched();
        }
        autoaim_node->autoaim_recorder.clearmode = 2;
        armor_detector.armor_type_recorder.clear();
        target_queue.clear();
    }
    bool pnp_flag = true;
    // armor_detector.armorFound=true;
    // RCLCPP_INFO(autoaim_node->get_logger(), "644");
    if (!armor_detector.armorFound)
    {
        armor_predictor.target_queue.clear();
    }
    else
    {
        // 找到目标,更新卡尔曼滤波器和反陀螺描述器wheel2land
        Target target;
        // RCLCPP_INFO(this->get_logger(), "???");
        // RCLCPP_INFO(autoaim_node->get_logger(), "650");
        pnp_flag = target_solver.armor_getCoord(armor_detector.target_armor, armor_detector.target_armor_deputy, target, pose, armor_num);
        // std::cout<<"x:"<<target.x<<std::endl;
        // std::cout<<"y:"<<target.y<<std::endl;
        // std::cout<<"z:"<<target.z<<std::endl;
        // if (!pnp_flag)
        // {
        //     target = last_target;
        //     RCLCPP_ERROR(autoaim_node->get_logger(), "pnp failed");
        //     //                LOGE("pnp failed");
        // }
        // if (target.z < 0)
        //     return;
        Util::wheel2land(target, pose); /* 转换到大地坐标系 */

        autoaim_node->autoaim_recorder.target_land_x = target.x;
        autoaim_node->autoaim_recorder.target_land_y = target.y;
        autoaim_node->autoaim_recorder.target_land_z = target.z;
        if (!USE_COMM)
        {
            if (DEBUG)
            {
                // cv::putText(testimg, "target_x_land: " + std::to_string(target.x), cv::Point(20, 380), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0));
                // cv::putText(testimg, "target_y_land: " + std::to_string(target.y), cv::Point(20, 430), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0));
                // cv::putText(testimg, "target_z_land: " + std::to_string(target.z), cv::Point(20, 480), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0));
            }
        }

        // RCLCPP_INFO(autoaim_node->get_logger(), "03");
        /*对获取的数据z坐标进行限制*/
        if (target_queue.isEmpty())
        {
            target_queue.push(target);
        }
        else
        {
            int i = -1;
            for (i = 1; i <= target_queue.length; i++)
            {
                Target target_temp;
                target_queue.back(target_temp, i);
                if (std::abs(target.z - target_temp.z) < 0.4)
                {
                    break;
                }
            }
            target_queue.push(target);
            if (i > target_queue.length)
            {
                return;
            }
        }

        // 解决转头bug1.类型
        // if (target.z * last_target.z < 0) // TODO
        // {
        //     if (Util::JudgeZDelta(target, last_target)) // 新解算的对象和上一帧对象z值偏差是否异常
        //     {
        //         target = last_target;
        //     }
        // }

        //            DEBUG(show_target_axis(image_original, target););
        target.xy_plane_distance = sqrt(pow(target.x, 2) + pow(target.y, 2));
        autoaim_node->tmp_xy_dis = target.xy_plane_distance;
        target.xyz_distance = sqrt(pow(target.x, 2) + pow(target.y, 2) + pow(target.z, 2));
        target.clock = pose.mcu_time; /* 给坐标以时钟,时间单位注意是ms*/
        target.yaw = Util::getYawForPTZ(target);
        target.pitch = Util::getPitchForPTZ(target);
        target.pose = pose;
        if (!USE_COMM)
        {
            if (DEBUG)
            {
                // cv::putText(testimg, "target_yaw: " + std::to_string(target.yaw), cv::Point(20, 280), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0));
                // cv::putText(testimg, "target_pitch: " + std::to_string(target.pitch), cv::Point(20, 330), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0));
            }
        }
        // RCLCPP_INFO(autoaim_node->get_logger(), "04");
        autoaim_node->autoaim_recorder.target_xy_plane_distance = target.xy_plane_distance;
        autoaim_node->autoaim_recorder.target_xyz_distance = target.xyz_distance;
        autoaim_node->autoaim_recorder.target_yaw = target.yaw;
        autoaim_node->autoaim_recorder.target_pitch = target.pitch;
        // geometry_msgs::msg::PoseStamped pose_msg;
        // pose_msg.header.stamp = autoaim_node->now();
        // pose_msg.pose.position.x = target.yaw;

        // Target target_temp;
        // target_queue.back(target_temp,1);  //i=1
        // cout<<"last_distance :"<<target_temp.xy_plane_distance<<endl;
        // double delta_dis =std::abs(target.xy_plane_distance-target_temp.xy_plane_distance);

        // TODO
        //  double delta_dis =std::abs(target.xy_plane_distance-last_target.xy_plane_distance);
        //  //如果与上一帧xy_plane_distance的差值大于一定的范围，那么将当前target的距离赋值为上一帧的对象的距离。
        //  if((delta_dis>dis_tolerance*last_target.xy_plane_distance) && delta_dis<10*last_target.xy_plane_distance && !armor_detector.flag_track_car_failed)
        //  {
        //      cout<<"distance aborted!"<<endl;
        //      target.xy_plane_distance=last_target.xy_plane_distance;
        //  }

        // DEBUG(
        //     cv::putText(image_original, "bullet_speed: " + std::to_string(work_msg.bullet_speed), cv::Point(20, 160), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //     cv::putText(image_original, "target_XY: " + std::to_string(target.xy_plane_distance), cv::Point(20, 220), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //     cv::putText(image_original, "msg,yaw: " + std::to_string(work_msg.ptz_yaw), cv::Point(20, 260), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));

        // );

        delta_time = (target.clock - last_target.clock) * 0.001;
        target.xy_v = (target.xy_plane_distance - last_target.xy_plane_distance) / delta_time;
        target.xyz_v = (target.xyz_distance - last_target.xyz_distance) / delta_time;
        target.yaw_v = (target.yaw - last_target.yaw) / delta_time;
        target.pitch_v = (target.pitch - last_target.pitch) / delta_time;
        // target.pitch_v = 0;

        last_target = target;

        // 画角度值
        // DEBUG(show_target_angle(image_original, target.yaw, target.pitch););
        // save_target_data(target, work_msg.bullet_speed); // xty出来挨打，程序中直接绝对路径！！！
        // RCLCPP_INFO(autoaim_node->get_logger(), "05");
        if (armor_detector.flag_switch_armor)
        {
            armor_predictor.target_queue.clear();
            armor_predictor.target_queue.push(target);
        }
        armor_predictor.target_queue.push(target);

        armor_predictor.ekf.armor_num = armor_num;
        //如果要插值，考虑从这里附近分割
        armor_predictor.update(target, work_msg.bullet_speed, armor_detector.flag_switch_armor);

        // RCLCPP_INFO(autoaim_node->get_logger(), "06");
        if (1 == mode)
        {
            // 更新反陀螺描述器
            InputOfGyro gyro_data;
            gyro_data.target = target;
            // 将yaw设置成为卡尔曼滤波器中的yaw数值
            gyro_data.target.yaw = armor_predictor.descriptior.posterior_yaw;
            gyro_data.armor_angle = armor_detector.target_armor.armor_angle;
            gyro_data.armor_switched = armor_detector.flag_switch_armor;
            gyro_data.yaw_speed = armor_predictor.descriptior.yaw_velocity;
            gyro_data.bullet_speed = work_msg.bullet_speed;
            gyro_data.isBigArmor = armor_detector.target_armor.isBigArmor;
            gyro_data.is_balance_infantry = armor_detector.is_balance_infantry;
            anti_gyror.update(gyro_data);
        }
    }
    // 在canvas上添加一些计算出的陀螺信息
    anti_gyror.show_result(testimg, work_msg.bullet_speed, time_process);
    // RCLCPP_INFO(autoaim_node->get_logger(), "07");
    // 以下代码，为了观察当前值,预测值以及过去的子弹在当前时刻的落点，比赛时需要注释
    // DEBUG(
    if (!USE_COMM)
    {
        if (DEBUG)
        {
            bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(pose.mcu_time);
            if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO))
            {
                SendPack send_pack_;
                send_pack_.burstShoot = true;
                anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                if (this->car_id == 5 && anti_gyror.get_average_z_distancce() > 0.5)
                {
                    send_pack_.pred_pitch += 0.3;
                }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);

                posterior_target_.yaw = send_pack_.pred_yaw + 90; // 转化成为视觉的yaw数值范围
                // 将预测的目标落点显示在图像上面
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
            }
            else
            {
                SendPack send_pack_;
                armor_predictor.predTarget(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                if (this->car_id == 5 && armor_predictor.descriptior.curr_target.z > 0.5)
                {
                    send_pack_.pred_pitch += 0.3;
                }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
                // armor_predictor.burst_shootstd::shared_ptr<interfaces::srv::Armor::Request> armors_in
            }
        }
        if (!testimg.empty())
        {
            // cv::Mat showimg;
            // cv::resize(testimg,showimg,cv::Size(testimg.cols/2,testimg.rows/2));
            cv::imshow("debug_autoaim", testimg);
            cv::waitKey(1);
            //     sensor_msgs::msg::Image msg_temp;
            // cv_bridge::CvImage cv_temp;
            // // 从mat转换到cvimage
            // cv_temp.encoding = "bgr8";
            // cv_temp.image = testimg;
            // // 从cvimage转化为msg并发布
            // cv_temp.toImageMsg(msg_temp);
            // autoaim_node->pub_img->publish(msg_temp);
        }
    }
}

// 旋转击打前哨站代码
void AutoAim::dynamic_follow_hit_outpost(void)
{
    //
    ArmorDir dir = ArmorDir::UNKNOWN;
    // 当数据存储器有效之后就可以进行旋转方向的使用
    if (dynamic_hit_outposter.outpost_description.valid)
    {
        dir = dynamic_hit_outposter.outpost_description.dir;
    }
    work_msg.enemy_number = 7;
    armor_detector.run_dynamic(armors, work_msg, dir, pose, armor_predictor.ekf_yaw_v);
    armor_predictor.get_tracker_state(armor_detector.curr_state, armor_detector.next_state);
    if (armor_detector.flag_track_car_failed || searching == true)
    {
        dynamic_hit_outposter.clear_if_car_switched();
        armor_detector.armor_type_recorder.clear();
        target_queue.clear();
        // 只有按下右键
        if (searching == true)
            dynamic_hit_outposter.outpost_description.valid = false;
        searching = false;
    }
    Target last_target;
    if (armor_detector.armorFound)
    {
        // 找到目标,更新卡尔曼滤波器和反陀螺描述器wheel2land

        Target target;
        armor_detector.target_armor.isBigArmor = false;
        target_solver.armor_getCoord(armor_detector.target_armor, armor_detector.target_armor_deputy, target, pose, armor_num);
        // save_pose_data(curr_image_object,target);
        // std::cout<<"target camera : "<<target.x<<" "<<target.y<<" "<<target.z<<std::endl;
        // std::cout<<"stm angle : "<<curr_image_object.pose.ptz_yaw<<" "<<curr_image_object.pose.ptz_pitch<<std::endl;

        // 解决1类型转头bug
        if (target.z * last_target.z < 0) // TODO
        {
            if (Util::JudgeZDelta(target, last_target)) // 新解算的对象和上一帧对象z值偏差是否异常
            {
                target = last_target;
            }
        }

        Util::wheel2land(target, pose); /* 转换到大地坐标系 */
        autoaim_node->autoaim_recorder.target_land_x = target.x;
        autoaim_node->autoaim_recorder.target_land_y = target.y;
        autoaim_node->autoaim_recorder.target_land_z = target.z;
        // target.z-=0.045;
        // std::cout<<"target world : "<<target.x<<" "<<target.y<<" "<<target.z<<std::endl;

        // DEBUG(cv::putText(image_original, "work_msg: " + std::to_string(work_msg.bullet_speed), cv::Point(20, 160), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //       cv::putText(image_original, "work_msg.ptz_yaw: " + std::to_string(work_msg.ptz_yaw), cv::Point(20, 180), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //       cv::putText(image_original, "ROLL: " + std::to_string(curr_image_object.pose.ptz_roll), cv::Point(20, 200), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0)););
        // save_pose_data(curr_image_object.pose,target);
        /*对获取的数据z坐标进行限制*/
        if (target_queue.isEmpty())
        {
            target_queue.push(target);
        }
        else
        {
            int i = -1;
            for (i = 1; i <= target_queue.length; i++)
            {
                Target target_temp;
                target_queue.back(target_temp, i);
                if (std::abs(target.z - target_temp.z) < 0.4)
                {
                    break;
                }
            }
            target_queue.push(target);
            if (i > target_queue.length)
            {
                return;
            }
        }

        // DEBUG(show_target_axis(image_original, target););
        target.xy_plane_distance = sqrt(pow(target.x, 2) + pow(target.y, 2));
        target.xyz_distance = sqrt(pow(target.x, 2) + pow(target.y, 2) + pow(target.z, 2));
        target.clock = pose.mcu_time; /* 给坐标以时钟,时间单位注意是ms*/
        target.yaw = Util::getYawForPTZ(target);
        // get_exporter("debug_yaw.txt")->export_info(target.yaw);
        // auto target_pitch_ = Util::getPitchForPTZUnderGravity(target, work_msg.bullet_speed);
        // auto target_pitch_ = Util::getPitchForPTZ(target);
        // target.pitch = target_pitch_.value_or(target.pitch);
        target.pitch = Util::getPitchForPTZ(target);
        target.pose = pose;
        // DEBUG(cv::putText(image_original, "target_xy:" + std::to_string(target.xy_plane_distance), cv::Point(20, 220), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0)););

        autoaim_node->autoaim_recorder.target_xy_plane_distance = target.xy_plane_distance;
        autoaim_node->autoaim_recorder.target_xyz_distance = target.xyz_distance;
        autoaim_node->autoaim_recorder.target_yaw = target.yaw;
        autoaim_node->autoaim_recorder.target_pitch = target.pitch;

        delta_time = (target.clock - last_target.clock) * 0.001;
        target.xy_v = (target.xy_plane_distance - last_target.xy_plane_distance) / delta_time;
        target.xyz_v = (target.xyz_distance - last_target.xyz_distance) / delta_time;
        target.yaw_v = (target.yaw - last_target.yaw) / delta_time;
        target.pitch_v = (target.pitch - last_target.pitch) / delta_time;

        last_target = target;

        armor_predictor.ekf.armor_num = armor_num;
        armor_predictor.update(target, work_msg.bullet_speed, armor_detector.flag_switch_armor);

        // 更新反陀螺描述器
        // InputOfGyro gyro_data;
        InputOfDynamicOutpost gyro_data;
        gyro_data.target = target;
        // 将yaw设置成为卡尔曼滤波器中的yaw数值
        //  gyro_data.target.yaw = armor_predictor.descriptior.posterior_yaw;

        gyro_data.armor_angle = armor_detector.target_armor.armor_angle;
        gyro_data.armor_switched = armor_detector.flag_switch_armor;
        gyro_data.yaw_speed = armor_predictor.descriptior.yaw_velocity;
        gyro_data.bullet_speed = work_msg.bullet_speed;
        gyro_data.isBigArmor = false;
        gyro_data.is_balance_infantry = false;
        // std::cout<<gyro_data.target.yaw<<std::endl;

        dynamic_hit_outposter.update_new_dynatic(gyro_data);
    }
    // // 在canvas上添加一些计算出的陀螺信息
    // DEBUG(anti_gyror.show_result(image_original, work_msg.bullet_speed, curr_image_object.pose.mcu_time););
    // anti_gyror.show_result(testimg, work_msg.bullet_speed, time_process);

    if (!USE_COMM)
    {
        if (DEBUG)
        {
            bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(pose.mcu_time);
            if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO))
            {
                SendPack send_pack_;
                send_pack_.burstShoot = true;
                anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                // if (this->car_id == 5 && anti_gyror.get_average_z_distancce() > 0.5)
                // {
                //     send_pack_.pred_pitch += 0.3;
                // }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);

                posterior_target_.yaw = send_pack_.pred_yaw + 90; // 转化成为视觉的yaw数值范围
                // 将预测的目标落点显示在图像上面
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
            }
            else
            {
                SendPack send_pack_;
                armor_predictor.predTarget(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                // if (this->car_id == 5 && armor_predictor.descriptior.curr_target.z > 0.5)
                // {
                //     send_pack_.pred_pitch += 0.3;
                // }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
                // armor_predictor.burst_shootstd::shared_ptr<interfaces::srv::Armor::Request> armors_in
            }
        }
        if (!testimg.empty())
        {
            // cv::Mat showimg;
            // cv::resize(testimg, showimg, cv::Size(testimg.cols / 2, testimg.rows / 2));
            cv::imshow("debug_autoaim", testimg);
            cv::waitKey(1);
            //     sensor_msgs::msg::Image msg_temp;
            // cv_bridge::CvImage cv_temp;
            // // 从mat转换到cvimage
            // cv_temp.encoding = "bgr8";
            // cv_temp.image = testimg;
            // // 从cvimage转化为msg并发布
            // cv_temp.toImageMsg(msg_temp);
            // autoaim_node->pub_img->publish(msg_temp);
        }
    }
    // // 以下代码，为了观察当前值,预测值以及过去的子弹在当前时刻的落点，比赛时需要注释
    // DEBUG(
    //     bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(curr_image_object.pose.mcu_time);
    //     if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO)) {
    //         SendPack send_pack_;
    //         send_pack.burstShoot = true;
    //         anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, curr_image_object.pose.mcu_time, send_pack_);

    //         Target posterior_target_ = armor_predictor.descriptior.curr_target;// 主要是为了获得z轴高度
    //         // 单位度
    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;
    //         posterior_target_.pitch = send_pack_.pred_pitch;
    //         posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
    //         posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);

    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;//转化成为视觉的yaw数值范围
    //         //将预测的目标落点显示在图像上面
    //         armor_predictor.show_pred(armor_predictor.descriptior.curr_target, posterior_target_, curr_image_object.pose, work_msg.bullet_speed);
    //     } else {
    //         SendPack send_pack_;
    //         armor_predictor.predTarget(work_msg.bullet_speed, curr_image_object.pose.mcu_time, send_pack_);
    //         Target posterior_target_ = armor_predictor.descriptior.curr_target;// 主要是为了获得z轴高度
    //         // 单位度
    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;
    //         posterior_target_.pitch = send_pack_.pred_pitch;
    //         posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
    //         posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
    //         armor_predictor.show_pred(armor_predictor.descriptior.curr_target, posterior_target_, curr_image_object.pose, work_msg.bullet_speed);
    //         // armor_predictor.burst_shoot(curr_image_object.pose, work_msg.bullet_speed, armor_detector.old_armor.armor_rect);
    //     }
    // );
}

void AutoAim::fix_hit_outpost(void)
{

    //
    ArmorDir dir = ArmorDir::UNKNOWN;
    // 当数据存储器有效之后就可以进行旋转方向的使用
    if (dynamic_hit_outposter.outpost_description.valid)
    {
        dir = dynamic_hit_outposter.outpost_description.dir;
    }
    work_msg.enemy_number = 7;
    armor_detector.run_dynamic(armors, work_msg, dir, pose, armor_predictor.ekf_yaw_v);
    armor_predictor.get_tracker_state(armor_detector.curr_state, armor_detector.next_state);
    if (armor_detector.flag_track_car_failed || searching == true)
    {
        dynamic_hit_outposter.clear_if_car_switched();
        armor_detector.armor_type_recorder.clear();
        target_queue.clear();
        if (searching == true)
            dynamic_hit_outposter.outpost_description.valid = false;
        searching = false;
    }
    // std::cout<<curr_image_object.pose.ptz_yaw<<",,,"<<curr_image_object.pose.ptz_pitch<<",,,"<<curr_image_object.pose.ptz_roll<<std::endl;
    Target last_target;
    if (armor_detector.armorFound)
    {
        // 找到目标,更新卡尔曼滤波器和反陀螺描述器wheel2land

        Target target;
        armor_detector.target_armor.isBigArmor = false;
        target_solver.armor_getCoord(armor_detector.target_armor, armor_detector.target_armor_deputy, target, pose, armor_num);
        // if (target.z < 0)
        //     return;
        Util::wheel2land(target, pose); /* 转换到大地坐标系 */

        // target_solver.armor_getCoord(armor_detector.target_armor, target);

        // save_pose_data(curr_image_object,target);
        // std::cout<<"target camera : "<<target.x<<" "<<target.y<<" "<<target.z<<std::endl;
        // std::cout<<"stm angle : "<<curr_image_object.pose.ptz_yaw<<" "<<curr_image_object.pose.ptz_pitch<<std::endl;

        // 解决1类型转头bug
        if (target.z * last_target.z < 0) // TODO
        {
            if (Util::JudgeZDelta(target, last_target)) // 新解算的对象和上一帧对象z值偏差是否异常
            {
                target = last_target;
            }
        }

        autoaim_node->autoaim_recorder.target_land_x = target.x;
        autoaim_node->autoaim_recorder.target_land_y = target.y;
        autoaim_node->autoaim_recorder.target_land_z = target.z;

        // Util::wheel2land(target, curr_image_object.pose); /* 转换到大地坐标系 */
        // target.z-=0.045;
        // std::cout<<"target world : "<<target.x<<" "<<target.y<<" "<<target.z<<std::endl;

        // DEBUG(cv::putText(image_original, "work_msg: " + std::to_string(work_msg.bullet_speed), cv::Point(20, 160), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //       cv::putText(image_original, "work_msg.ptz_yaw: " + std::to_string(work_msg.ptz_yaw), cv::Point(20, 180), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //       cv::putText(image_original, "ROLL: " + std::to_string(curr_image_object.pose.ptz_roll), cv::Point(20, 200), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        //       cv::putText(image_original, "z: " + std::to_string(target.z), cv::Point(20, 100), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0)););
        // save_pose_data(curr_image_object.pose,target);
        /*对获取的数据z坐标进行限制*/
        if (target_queue.isEmpty())
        {
            target_queue.push(target);
        }
        else
        {
            int i = -1;
            for (i = 1; i <= target_queue.length; i++)
            {
                Target target_temp;
                target_queue.back(target_temp, i);
                if (std::abs(target.z - target_temp.z) < 0.4)
                {
                    break;
                }
            }
            target_queue.push(target);
            if (i > target_queue.length)
            {
                return;
            }
        }

        // DEBUG(show_target_axis(image_original, target););
        target.xy_plane_distance = sqrt(pow(target.x, 2) + pow(target.y, 2));
        target.xyz_distance = sqrt(pow(target.x, 2) + pow(target.y, 2) + pow(target.z, 2));
        target.clock = pose.mcu_time; /* 给坐标以时钟,时间单位注意是ms*/
        target.yaw = Util::getYawForPTZ(target);
        // get_exporter("debug_yaw.txt")->export_info(target.yaw);
        // auto target_pitch_ = Util::getPitchForPTZUnderGravity(target, work_msg.bullet_speed);
        // auto target_pitch_ = Util::getPitchForPTZ(target);
        // target.pitch = target_pitch_.value_or(target.pitch);
        target.pitch = Util::getPitchForPTZ(target);
        target.pose = pose;
        // DEBUG(cv::putText(image_original, "target_xy:" + std::to_string(target.xy_plane_distance), cv::Point(20, 220), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0)););

        autoaim_node->autoaim_recorder.target_xy_plane_distance = target.xy_plane_distance;
        autoaim_node->autoaim_recorder.target_xyz_distance = target.xyz_distance;
        autoaim_node->autoaim_recorder.target_yaw = target.yaw;
        autoaim_node->autoaim_recorder.target_pitch = target.pitch;

        // for v-params tuning
        delta_time = (target.clock - last_target.clock) * 0.001;
        target.xy_v = (target.xy_plane_distance - last_target.xy_plane_distance) / delta_time;
        target.xyz_v = (target.xyz_distance - last_target.xyz_distance) / delta_time;
        target.yaw_v = (target.yaw - last_target.yaw) / delta_time;
        target.pitch_v = (target.pitch - last_target.pitch) / delta_time;

        last_target = target;

        // 画角度值
        // DEBUG(show_target_angle(image_original, target.yaw, target.pitch););
        // save_target_data(target);//xty出来挨打，程序中直接绝对路径！！！

        armor_predictor.ekf.armor_num = armor_num;
        armor_predictor.update(target, work_msg.bullet_speed, armor_detector.flag_switch_armor);

        // 更新反陀螺描述器
        // InputOfGyro gyro_data;
        InputOfDynamicOutpost gyro_data;
        gyro_data.target = target;

        // std::cout<<target.yaw<<std::endl;
        // 将yaw设置成为卡尔曼滤波器中的yaw数值
        // gyro_data.target.yaw = armor_predictor.descriptior.posterior_yaw;

        gyro_data.armor_angle = armor_detector.target_armor.armor_angle;
        gyro_data.armor_switched = armor_detector.flag_switch_armor;
        gyro_data.yaw_speed = armor_predictor.descriptior.yaw_velocity;
        gyro_data.bullet_speed = work_msg.bullet_speed;
        gyro_data.isBigArmor = false;
        gyro_data.is_balance_infantry = false;
        // std::cout<<gyro_data.target.yaw<<std::endl;

        dynamic_hit_outposter.update_new(gyro_data);
    }
    // anti_gyror.show_result(testimg, work_msg.bullet_speed, time_process);

    if (!USE_COMM)
    {
        if (DEBUG)
        {
            bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(pose.mcu_time);
            if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO))
            {
                SendPack send_pack_;
                send_pack_.burstShoot = true;
                anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                // if (this->car_id == 5 && anti_gyror.get_average_z_distancce() > 0.5)
                // {
                //     send_pack_.pred_pitch += 0.3;
                // }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);

                posterior_target_.yaw = send_pack_.pred_yaw + 90; // 转化成为视觉的yaw数值范围
                // 将预测的目标落点显示在图像上面
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
            }
            else
            {
                SendPack send_pack_;
                armor_predictor.predTarget(work_msg.bullet_speed, pose.mcu_time, send_pack_);
                // if (this->car_id == 5 && armor_predictor.descriptior.curr_target.z > 0.5)
                // {
                //     send_pack_.pred_pitch += 0.3;
                // }
                Target posterior_target_ = armor_predictor.descriptior.curr_target; // 主要是为了获得z轴高度
                // 单位度
                posterior_target_.yaw = send_pack_.pred_yaw + 90;
                posterior_target_.pitch = send_pack_.pred_pitch;
                posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
                posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
                armor_predictor.show_pred(testimg, armor_predictor.descriptior.curr_target, posterior_target_, pose, work_msg.bullet_speed);
                // armor_predictor.burst_shootstd::shared_ptr<interfaces::srv::Armor::Request> armors_in
            }
        }
        if (!testimg.empty())
        {
            // cv::Mat showimg;
            // cv::resize(testimg, showimg, cv::Size(testimg.cols / 2, testimg.rows / 2));
            cv::imshow("debug_autoaim", testimg);
            cv::waitKey(1);
            //     sensor_msgs::msg::Image msg_temp;
            // cv_bridge::CvImage cv_temp;
            // // 从mat转换到cvimage
            // cv_temp.encoding = "bgr8";
            // cv_temp.image = testimg;
            // // 从cvimage转化为msg并发布
            // cv_temp.toImageMsg(msg_temp);
            // autoaim_node->pub_img->publish(msg_temp);
        }
    }
    // // 在canvas上添加一些计算出的陀螺信息
    // DEBUG(anti_gyror.show_result(image_original, work_msg.bullet_speed, curr_image_object.pose.mcu_time););

    // // 以下代码，为了观察当前值,预测值以及过去的子弹在当前时刻的落点，比赛时需要注释
    // DEBUG(
    //     bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(curr_image_object.pose.mcu_time);
    //     if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO)) {
    //         SendPack send_pack_;
    //         send_pack.burstShoot = true;
    //         anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, curr_image_object.pose.mcu_time, send_pack_);

    //         Target posterior_target_ = armor_predictor.descriptior.curr_target;// 主要是为了获得z轴高度
    //         // 单位度
    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;
    //         posterior_target_.pitch = send_pack_.pred_pitch;
    //         posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
    //         posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);

    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;//转化成为视觉的yaw数值范围
    //         //将预测的目标落点显示在图像上面
    //         armor_predictor.show_pred(armor_predictor.descriptior.curr_target, posterior_target_, curr_image_object.pose, work_msg.bullet_speed);
    //     } else {
    //         SendPack send_pack_;
    //         armor_predictor.predTarget(work_msg.bullet_speed, curr_image_object.pose.mcu_time, send_pack_);
    //         Target posterior_target_ = armor_predictor.descriptior.curr_target;// 主要是为了获得z轴高度
    //         // 单位度
    //         posterior_target_.yaw = send_pack_.pred_yaw + 90;
    //         posterior_target_.pitch = send_pack_.pred_pitch;
    //         posterior_target_.x = armor_predictor.descriptior.pre_xy_plane_distance * cos(posterior_target_.yaw * Util::PI / 180.0);
    //         posterior_target_.y = armor_predictor.descriptior.pre_xy_plane_distance * sin(posterior_target_.yaw * Util::PI / 180.0);
    //         armor_predictor.show_pred(armor_predictor.descriptior.curr_target, posterior_target_, curr_image_object.pose, work_msg.bullet_speed);
    //         // armor_predictor.burst_shoot(curr_image_object.pose, work_msg.bullet_speed, armor_detector.old_armor.armor_rect);
    //     }
    // );
}

void AutoAim::setArmorPredictdata(SendPack &send_pack, int64_t clock_now)
{
    send_pack.mode = Mode::MODE_ARMOR;
    send_pack.clock = clock_now;
    send_pack.burstShoot = false;
    send_pack.fire = false;

    bool if_use_ekf = false;
    bool static_hit = false;
    double static_hit_range = 0;

    if (armor_predictor.queue_ekf_omega.is_full())
    {
        double avg_omega = 0;
        for (int i = 0; i < armor_predictor.queue_ekf_omega.num_of_elem(); ++i)
        {
            avg_omega += armor_predictor.queue_ekf_omega[i];
        }

        avg_omega /= armor_predictor.queue_ekf_omega.num_of_elem();//计算近期的平均角速度
        if (std::abs(avg_omega) < 45 * M_PI / 180.0)//如果角速度很小，则使用kf的结果
        {
            if_use_ekf = false;
            static_hit = false;
        }
        else if (std::abs(avg_omega) < 315 * M_PI / 180)//如果角速度中等，则用ekf的结果动态击打
        {
            if_use_ekf = true;
            static_hit = false;
        }
        else//如果角速度很大，则静止击打
        {
            if_use_ekf = true;
            static_hit = true;
            static_hit_range = 20 * M_PI / 180;
        }

        if ((1 == car_id || 6 == car_id) && (4 == mode || 5 == mode))//英雄打前哨站还是用静止击打吧
        {
            static_hit = true;
        }
    }

    if (if_use_ekf)
    {
        double hit_range = 70 * M_PI / 180;//击打区间
        armor_predictor.ekf_predTarget(work_msg.bullet_speed, clock_now, send_pack, hit_range, static_hit, static_hit_range);
    }
    else
    {
        armor_predictor.predTarget(work_msg.bullet_speed, clock_now, send_pack);
        send_pack.fire = true;
    }
    // bool anti_gyro_description_updated = anti_gyror.If_gyro_description_updated(clock_now);
    // //  根据当前敌方装甲板状态选择合适的击打策略
    // if (anti_gyro_description_updated && (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::HIGH_SPEED_OR_FAR_GYRO))
    // {
    //     autoaim_node->autoaim_recorder.is_antigyro = 1;
    //     send_pack.burstShoot = true;
    //     anti_gyror.high_or_far_gyro_predict(work_msg.bullet_speed, clock_now, send_pack);
    //     // send_pack.pred_pitch += 0.3;
    //     if (armor_detector.is_balance_infantry)
    //     {
    //         send_pack.pred_pitch += 0.5;
    //     }
    //     send_pack.pred_pitch -= 0.3;
    // }
    // else
    // {
    //     // autoaim_node->autoaim_recorder.is_antigyro = 0;
    //     // if (10 > work_msg.bullet_speed || 30 < work_msg.bullet_speed)
    //     // {
    //     //     work_msg.bullet_speed = 22;
    //     // }
    //     armor_predictor.predTarget(work_msg.bullet_speed, clock_now, send_pack);
    //     //std::cout<<"SPEED:"<<work_msg.bullet_speed<<std::endl;
    //     if (anti_gyror.gyro_description.armor_status == ArmorMotionStatus::NO_GYRO)
    //     {
    //         send_pack.burstShoot = armor_predictor.burst_shoot(pose, work_msg.bullet_speed, armor_detector.old_armor.armor_rect);
    //     }
    //     send_pack.fire = true;
    // }

    


    // send_pack.fire = true;
    //   std::cout<<"send_pack.fire:"<<send_pack.fire<<std::endl;

    // send_pack.fire = false;
}

void AutoAim::setDynamicHitOutpostNew(SendPack &send_pack, int64_t clock_now)
{
    send_pack.mode = Mode::MODE_DYNAMIC_OUTPOST;
    send_pack.clock = clock_now;
    send_pack.burstShoot = false;
    send_pack.fire = false;
    // bool outpost_description_updated = dynamic_hit_outposter.if_outpost_description_updated(clock_now);
    // if (outpost_description_updated && (dynamic_hit_outposter.outpost_description.oupost_status == OutpostMotionStatus::GYRO)&&dynamic_hit_outposter.fireBegin==true)
    // {
    //     send_pack.burstShoot = true;
    //     dynamic_hit_outposter.aim_center_hit_outpost_new(work_msg.bullet_speed, clock_now, send_pack);
    // }
    // else
    // {
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    // }

    // send_pack.mode = Mode::MODE_DYNAMIC_OUTPOST;
    // send_pack.clock = clock_now;
    // send_pack.burstShoot = false;
    // send_pack.fire = false;

    // if (dynamic_hit_outposter.outpost_description.valid == true)
    // {
    //     // std::cout<<"true!!!!!"<<std::endl;
    //     dynamic_hit_outposter.follow_hit_outpost_new(work_msg.bullet_speed, clock_now, send_pack);
    //     // send_pack.fire=true;
    // }
    // else
    // {
    //     if (3 == car_id)
    //     {
    //         if (armor_detector.armorFound)
    //         {
    //             send_pack.target_found = true;
    //             send_pack.fire = false;
    //             InputOfDynamicOutpost tmp_outpost_data;
    //             dynamic_hit_outposter.car_armors_data.back(tmp_outpost_data, 1);
    //             Target tmp_target;
    //             tmp_target = tmp_outpost_data.target;
    //             send_pack.pred_yaw = tmp_target.yaw;
    //             send_pack.pred_pitch = tmp_target.pitch;
    //             send_pack.yaw_resolution = getYawResolution(tmp_target, true);
    //             send_pack.pitch_resolution = getPitchResolution(tmp_target);
    //             send_pack.yaw_palstance = 0;
    //             send_pack.pitch_palstance = 0;
    //         }
    //         else
    //         {
    //             send_pack.target_found = false;
    //             send_pack.fire = false;
    //         }
    //     }
    //     else
    //     {
    //         send_pack.target_found = false;
    //         send_pack.fire = false;
    //     }
    // }

    bool static_hit;
    double hit_range = 80 * M_PI / 180;
    double static_hit_range = 0;
    if ((1 == car_id || 6 == car_id) && (4 == mode || 5 == mode))
    {
        static_hit = true;
        static_hit_range = 6 * M_PI / 180;
    }
    else
    {
        static_hit = false;
    }
    // std::cout << "1986" << std::endl;
    armor_predictor.ekf_predTarget(work_msg.bullet_speed, clock_now, send_pack, hit_range, static_hit, static_hit_range);
    // bigarmor.csv
    /*--------------------------*/
    // std::ofstream fout;
    // std::string save_addr = "../save/csv/sendFire.csv";
    // fout.open(save_addr, std::ios::app | std::ios::out);
    // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    // fout << send_pack.target_found << ",";
    // fout << send_pack.fire << ",";
    // fout << send_pack.clock << ",";
    // fout << send_pack.pred_yaw << ",";
    // fout << send_pack.pred_pitch << std::endl;
    // fout.close();
    /*--------------------------*/
}

// 固定击打前哨站发送演算函数
void AutoAim::setFixHitOutpostNew(SendPack &send_pack, int64_t clock_now)
{
    send_pack.mode = Mode::MODE_FIX_OUTPOST;
    send_pack.clock = clock_now;
    send_pack.burstShoot = false;
    send_pack.fire = false;
    // bool outpost_description_updated = dynamic_hit_outposter.if_outpost_description_updated(clock_now);
    // if (outpost_description_updated && (dynamic_hit_outposter.outpost_description.oupost_status == OutpostMotionStatus::GYRO)&&dynamic_hit_outposter.fireBegin==true)
    // {
    //     send_pack.burstShoot = true;
    //     dynamic_hit_outposter.aim_center_hit_outpost_new(work_msg.bullet_speed, clock_now, send_pack);
    // }
    // else
    // {
    //     send_pack.target_found = false;
    //     send_pack.fire = false;
    // }

    bool static_hit;
    double hit_range = 80 * M_PI / 180;
    double static_hit_range = 0;
    if ((1 == car_id || 6 == car_id) && (4 == mode || 5 == mode))
    {
        static_hit = true;
        static_hit_range = 6 * M_PI / 180;
    }
    else
    {
        static_hit = false;
    }
    armor_predictor.ekf_predTarget(work_msg.bullet_speed, clock_now, send_pack, hit_range, static_hit, static_hit_range);
    // bigarmor.csv
    /*--------------------------*/
    // std::ofstream fout;
    // std::string save_addr = "../save/csv/sendFire.csv";
    // fout.open(save_addr, std::ios::app | std::ios::out);
    // if (!fout.is_open()) { std::cout << "unvalid save_addr" << std::endl; }
    // fout << send_pack.target_found << ",";
    // fout << send_pack.fire << ",";
    // fout << send_pack.clock << ",";
    // fout << send_pack.pred_yaw << ",";
    // fout << send_pack.pred_pitch << std::endl;
    // fout.close();
    /*--------------------------*/
}

void AutoAim::timer_callback()
{
    // tb.time = rclcpp::Clock().now().seconds()*1000.;
    this->pub_tobase->publish(this->tb);
    tb = interfaces::msg::Tobase();
}

std::shared_ptr<AutoAim> autoaim_node;
cv::Mat testimg;
int USE_COMM;
bool DEBUG = 1;
geometry_msgs::msg::PoseStamped pose_msg;

int main(int argc, char **argv)
{
    //    std::cout << PROJECT_DIR << std::endl;
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    autoaim_node = std::make_shared<AutoAim>("auto_aim");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(autoaim_node);
    // node->armorFunc();
    rclcpp::shutdown();
    return 0;
}
